from microbit import *
import pca9685
import math
MatrixInit = 0

class MatrixBit(object):
    def __init__(self, address = 0x40, freq = 50):
        global MatrixInit
        self.pca9685 = pca9685.PCA9685(i2c, address, freq, MatrixInit)
        if not MatrixInit:
            MatrixInit = 1

    def motor_run(self, index, speed, dir = 1):  # index:0~5,speed:-255~255,dir:-1, 1
        speed = speed * 16 * dir # map 255 to 4096
        if speed >= 4096:
            speed = 4095
        if speed <= -4096:
            speed = -4095

        if index > 5 or index < 0:
            return
        if index == 0:
            index2 = 1
        elif index == 1:
            index2 = 2
        elif index == 2:
            index2 = 8
        elif index == 3:
            index2 = 7
        elif index == 4:
            index2 = 3
        elif index == 5:
            index2 = 4

        pp = (index2 - 1) * 2
        pn = (index2 - 1) * 2 + 1
        if index2 == 1 or index2 == 8:
            if speed >= 0:
                self.pca9685.set_pwm(pp, 0, speed)
                self.pca9685.set_pwm(pn, 0, 0)
            else:
                self.pca9685.set_pwm(pp, 0, 0)
                self.pca9685.set_pwm(pn, 0, -speed)
        else:
            if speed >= 0:
                self.pca9685.set_pwm(pp, 0, 0)
                self.pca9685.set_pwm(pn, 0, speed)
            else:
                self.pca9685.set_pwm(pp, 0, -speed)
                self.pca9685.set_pwm(pn, 0, 0)

    def motor_stop(self, index):  # index:0~5
        self.motor_run(index, 0)
            
    def servo(self, index, degree):  # index:0~3, degree:0~180
        if index > 3 or index < 0:
            return

        value = (degree * 10 + 600) * 4096 // 20000
        self.pca9685.set_pwm(index + 8, 0, value)

    def stepper_set(self, index, dir):  # index:0~1, dir:-1, 1
        if index > 1 or index < 0:
            return

        if index == 0:
            if dir == 1:
                self.pca9685.set_pwm(0, 0, 1023)
                self.pca9685.set_pwm(1, 2047, 3071)
                self.pca9685.set_pwm(2, 1023, 2047)
                self.pca9685.set_pwm(3, 3071, 4095)
            else:
                self.pca9685.set_pwm(0, 3071, 4095)
                self.pca9685.set_pwm(1, 1023, 2047)
                self.pca9685.set_pwm(2, 2047, 3071)
                self.pca9685.set_pwm(3, 0, 1023)
        elif index == 1:
            if dir == 1:
                self.pca9685.set_pwm(4, 0, 1023)
                self.pca9685.set_pwm(5, 2047, 3071)
                self.pca9685.set_pwm(6, 1023, 2047)
                self.pca9685.set_pwm(7, 3071, 4095)
            else:
                self.pca9685.set_pwm(4, 3071, 4095)
                self.pca9685.set_pwm(5, 1023, 2047)
                self.pca9685.set_pwm(6, 2047, 3071)
                self.pca9685.set_pwm(7, 0, 1023)

    def stepper(self, index, degree):  # index:0~1, degree:-360~360
        self.stepper_set(index, degree > 0)
        degree = abs(degree)
        sleep(1380 * degree / 360)

        if index == 0:
            self.motor_stop(0)
            self.motor_stop(1)
        elif index == 1:
            self.motor_stop(4)
            self.motor_stop(5)
    
    def stepper2(self, index1, degree1, index2, degree2):  # index:0~1, degree:-360~360
        self.stepper_set(index1, degree1 > 0)
        self.stepper_set(index2, degree2 > 0)
        degree1 = abs(degree1)
        degree2 = abs(degree2)
        sleep(1380 * min(degree1, degree2) / 360)

        if degree1 > degree2:
            self.motor_stop(4)
            self.motor_stop(5)
            sleep(1380 * (degree1 - degree2) / 360)
        elif degree1 <= degree2:
            self.motor_stop(0)
            self.motor_stop(1)
            sleep(1380 * (degree2 - degree1) / 360)

        self.motor_stop(4)
        self.motor_stop(5)
        self.motor_stop(0)
        self.motor_stop(1)